#include "main.h"
#include "rs485.h"
#include "timer.h"
// #include <stdio.h>
#include <stdbool.h>
#include "printf.h"
// 如果中断程序接受到完整的数据包，但是尚未处理，该值为true
static volatile bool one_cmd_pkg_ready = false;
// 接收到数据包的时间（单片机上电时的时间计为0，每隔1us加1）
static volatile uint64_t uart_rx_time;
// 发送缓冲区
static uint8_t rs485_tx[17];
// 接收缓冲区
static uint8_t uart_rx_buffer[256];
// 设备编号
static int rs485_id = 1;
// 指令计数
static uint8_t rs485_cmd_count = 0;

// 使用DMA+USART发送固定长度的数据
static void uart_write_none_blocking(uint8_t *buffer, int length)
{
    // 1. 等待上次传输完成
    if(LL_DMA_IsEnabledChannel(DMA1, LL_DMA_CHANNEL_2)) {
        // 等待当前传输完成或出错
        while(!LL_DMA_IsActiveFlag_TC2(DMA1) && !LL_DMA_IsActiveFlag_TE2(DMA1));

        // 清除所有可能存在的标志
        LL_DMA_ClearFlag_TC2(DMA1);
        LL_DMA_ClearFlag_TE2(DMA1);

        // 禁用通道
        LL_DMA_DisableChannel(DMA1, LL_DMA_CHANNEL_2);
    }

    // 2. 配置DMA
    LL_DMA_SetDataLength(DMA1, LL_DMA_CHANNEL_2, length);
    LL_DMA_SetMemoryAddress(DMA1, LL_DMA_CHANNEL_2, (uint32_t)buffer);
    LL_DMA_SetPeriphAddress(DMA1, LL_DMA_CHANNEL_2, LL_USART_DMA_GetRegAddr(USART1, LL_USART_DMA_REG_DATA_TRANSMIT));

    // 3. 先使能DMA通道，再使能USART的DMA请求
    LL_DMA_EnableChannel(DMA1, LL_DMA_CHANNEL_2);
    LL_USART_EnableDMAReq_TX(USART1);
}

// 串口RX中断，根据FF FF包头同步，并且接收数据
// 例如 ff ff 07 01 01 00 4f
// 不管总线上的数据是否是需要的，都会进行接收，以免丢失同步信号
// 例如上一个数据包以FF结尾，下一个数据包FF FF开头，会出现同步异常
CCMRAM void USART_CharReception_Callback(uint8_t ch)
{
    const int WAIT_FIRST_FF = 0;
    const int WAIT_SECOND_FF = 1;
    const int WAIT_LENGTH = 2;
    uint8_t *p = uart_rx_buffer;

    static int stat = WAIT_FIRST_FF;

    if(stat == WAIT_FIRST_FF){
        if(ch == 0xFF){
            stat = WAIT_SECOND_FF;
        }else{
            stat = WAIT_FIRST_FF;
        }
    }else if(stat == WAIT_SECOND_FF){
        if(ch == 0xFF){
            stat = WAIT_LENGTH;
        }else{
            stat = WAIT_FIRST_FF;
        }
    }else if(stat == WAIT_LENGTH){
        if(one_cmd_pkg_ready){
            // 如果上一个数据包还没有处理完毕，丢弃这个包
            stat = WAIT_FIRST_FF;
        }else{
            if(ch >= 5 && ch <= 128){
                p[0] = 0xff;
                p[1] = 0xff;
                p[2] = ch;
                stat++;
            }else{
                stat = WAIT_FIRST_FF;
            }
        }

    }else if(stat > WAIT_LENGTH){
        if(stat < p[2] - 1){
            p[stat] = ch;
            stat++;
        }else if(stat == p[2] - 1){
            p[stat] = ch; // CRC
            one_cmd_pkg_ready = true;
            uart_rx_time = get_absolute_time();
            stat = WAIT_FIRST_FF;
        }
    }
}

// CRC8计算
// 此处使用软件CRC方案，避免和角度传感器的硬件CRC计算冲突
static uint8_t crc8(uint8_t *datagram, int datagramLength)
{
    int i, j;
    uint8_t currentByte;
    uint8_t crc = 0;
    for (i = 0; i < datagramLength; i++)
    {                              // Execute for all bytes of a message
        currentByte = datagram[i]; // Retrieve a byte to be sent from Array
        for (j = 0; j < 8; j++)
        {
            if ((crc >> 7) ^ (currentByte & 0x01)) // update CRC based result of XOR operation
            {
                crc = (crc << 1) ^ 0x07;
            }
            else
            {
                crc = (crc << 1);
            }
            currentByte = currentByte >> 1;
        } // for CRC bit
    }

    /* 复位CRC计算单元（加载初始值）*/
    //LL_CRC_ResetCRCCalculationUnit(CRC);

    /* 逐字节写入数据到CRC数据寄存器 */
    //for (int i = 0; i < datagramLength; i++) {
    //  LL_CRC_FeedData8(CRC, datagram[i]);
    //}
    /* 读取8位CRC结果 */
    //uint8_t crc = LL_CRC_ReadData8(CRC);
    return crc;
}


/*
    ===== 主机→电机控制器数据帧格式 =====
    [字头] + [数据长度] + [指令类型 + ID号] + [数据] + [CRC8]

    [01字头]             uint16_t 固定值0xFFFF（2字节）
    [2 数据长度]          uint8_t len（1字节），指数据包的总长度，含0xFFFF和校验和
    [3 指令类型 + ID号]   低4bit：取值范围1-4，0用于广播
                        高4bit：指令类型
    [n-1 数据]           uint8_t data[250] 最大250字节
    [n CRC8]            uint8_t 校验值（含字头）

    注：每个电机仅处理ID匹配的指令块，其他忽略
*/
typedef enum{
    RS485_CMD_STAT = 0,
    RS485_CMD_ENABLE = 1,
    RS485_CMD_DISABLE = 2,
    RS485_CMD_RESET = 3,
    RS485_CMD_CLEAR_FIFO = 4,
    RS485_CMD_SYNC = 5,
    RS485_CMD_FIFO = 6,
}rs485_cmd;

int decode_uart_rx_date(void)
{
    if(!one_cmd_pkg_ready){
        return 0;
    }
    uint8_t *p = uart_rx_buffer;
    const uint64_t RX_DELAY = 50; // us
    bool need_ack = false;

    int pkg_length = p[2];
    uint8_t crc = crc8(p, pkg_length - 1);
#ifdef DBG_PRINT_ORIG_DATE
    for(int i=0; i<pkg_length; i++){
        printf_("%02x ", p[i]);
    }
    printf_("\n");
#endif
    if(p[pkg_length - 1] != crc){
        printf_("rs485 crc error, crc = 0x%02X, p[pkg_length - 1]=0x%02X\n",
                crc, p[pkg_length - 1]);
        one_cmd_pkg_ready = false;
        return 1;
    }
    if(p[3] == 0xff){
        // 这个包是其他控制器发给主机的
        one_cmd_pkg_ready = false;
        return 2;
    }
    uint8_t id = p[3] & 0x0f;
    if(id != rs485_id && id != 0){
        // 这个包是发给其他控制器的
        one_cmd_pkg_ready = false;
        return 3;
    }
    rs485_cmd cmd = p[3] >> 4;
    // printf("id=%d, cmd=%d\n", id, cmd);
    if(cmd == RS485_CMD_STAT && pkg_length == 5){
        // ------------------ 状态查询指令 RS485_CMD_STAT -----------------------
        rs485_cmd_count ++;
        // 响应数据结构（固定12字节）：
        // 历史接收指令计数,超过255会回到0
        /*
        // 上一次查询到现在的最大控制误差
        uint16_t max_error = foc_ctrl.max_error;
        foc_ctrl.max_error = 0;
        // 运动控制器状态（高1bit）+ 堵转状态（次高1bit）+ FIFO已用空间（低14bit）
        uint16_t fifo_status = foc_ctrl.trap_run ? 0x8000 : 0;
        fifo_status |= foc_ctrl.block ? 0x4000 : 0;
        fifo_status |= fifo_count(&foc_fifo) & 0x3FFF;
        // 温度值（℃）
        int8_t temperature = roundf( read_onboard_temperature(foc_ctrl.temperature_raw) );
        // 当前位置（小端序）
        int32_t pos = foc_ctrl.multiple_angle_read;
        // 电源电压mV（小端序）
        int16_t voltage = roundf(1000.0f * VBUS_FACTOR * foc_ctrl.power_voltage_filter);

        uint8_t rs485_tx[5+12];
        rs485_tx[0] = 0xff;
        rs485_tx[1] = 0xff;
        rs485_tx[2] = sizeof(rs485_tx);
        rs485_tx[3] = 0xff; // 固定为0xff，表示表示电机控制器发给主机的数据
        rs485_tx[4] = cmd_count;
        rs485_tx[5] = max_error & 0xff;
        rs485_tx[6] = (max_error >> 8) & 0xff;
        rs485_tx[7] = fifo_status & 0xff;
        rs485_tx[8] = (fifo_status >> 8) & 0xff;
        rs485_tx[9] = temperature;
        rs485_tx[10] = pos & 0xff;
        rs485_tx[11] = (pos >> 8) & 0xff;
        rs485_tx[12] = (pos >> 16) & 0xff;
        rs485_tx[13] = (pos >> 24) & 0xff;
        rs485_tx[14] = voltage & 0xff;
        rs485_tx[15] = (voltage >> 8) & 0xff;
        rs485_tx[16] = crc8(rs485_tx, sizeof(rs485_tx) - 1);
        */
        // 测试数据
        const uint8_t pkg_size = 17;
        rs485_tx[0] = 0xff;
        rs485_tx[1] = 0xff;
        rs485_tx[2] = pkg_size;// 数据包长度为17
        rs485_tx[3] = 0xff; // 固定为0xff，表示表示电机控制器发给主机的数据
        rs485_tx[4] = rs485_cmd_count;
        rs485_tx[5] = 11;
        rs485_tx[6] = 0;
        rs485_tx[7] = 0;
        rs485_tx[8] = 0;
        rs485_tx[9] = 23;
        rs485_tx[10] = 53;
        rs485_tx[11] = 0;
        rs485_tx[12] = 0;
        rs485_tx[13] = 0;
        rs485_tx[14] = 23;
        rs485_tx[15] = 12;
        rs485_tx[16] = crc8(rs485_tx, pkg_size - 1);

        // 等待足够长的时间，以便主机完成485方向切换
        sleep_until(uart_rx_time + RX_DELAY);
        uart_write_none_blocking(rs485_tx, pkg_size);
        need_ack = false;
    }else if(cmd == RS485_CMD_ENABLE && pkg_length == 5){
        // 使能电机
        rs485_cmd_count ++;
        //foc_ctrl.multiple_angle_target = foc_ctrl.multiple_angle_read;
        //foc_ctrl.mode = MODE_POSITION_WITH_TRAPEZOID;
        need_ack = true;
    }else if(cmd == RS485_CMD_DISABLE && pkg_length == 5){
        // 禁用电机
        rs485_cmd_count ++;
        //foc_ctrl.mode = MODE_CURRENT;
        //sleep_us(500);// 等待至少1个控制周期，防止设置iq、id后又被修改
        //foc_ctrl.iq_target = 0;
        //foc_ctrl.id_target = 0;
        // trap_first_run = true;
        need_ack = true;
    }else if(cmd == RS485_CMD_RESET && pkg_length == 5){
        // 复位控制板
        rs485_cmd_count ++;
        //watchdog_enable(10, 1);
        need_ack = true;
    }else if(cmd == RS485_CMD_CLEAR_FIFO && pkg_length == 5){
        //fifo_clear(&foc_fifo);
        rs485_cmd_count ++;
        need_ack = true;
    }else if(cmd == RS485_CMD_SYNC && pkg_length == 1+5){
        // 判断是否要开启这个电机的同步控制
        // 如果想同步控制电机1和电机2，发送(1<<1)|(1<<2)即可
        // P[4] bit 7 : 堵转时是否暂停，==1暂停
        // P[4] bit 4 : 控制电机4
        // P[4] bit 3 : 控制电机3
        // P[4] bit 2 : 控制电机2
        // P[4] bit 1 : 控制电机1
        if( (p[4] & (1<<rs485_id)) ){
            // p[4]的最高位，表示堵转时是否要暂停运动控制
            //foc_ctrl.stop_when_block         = p[4] & (1<<7) ? true : false;
            //foc_ctrl.max_error = 0;
            //foc_ctrl.block = false;
            //foc_ctrl.trap_run = true;
            need_ack = true;
        }else{
            need_ack = false;
        }
        // printf("RS485_CMD_SYNC\n");
        rs485_cmd_count ++;

    }else if(cmd == RS485_CMD_FIFO && pkg_length >= 8+5){
//        int succ = decompress_and_write_fifo(&p[4], pkg_length - 5, &foc_fifo);
//        // printf("RS485_CMD_FIFO\n");
//        if(succ == 0){
//            rs485_cmd_count ++;
//            need_ack = true;
//        }else{
//            printf("decompress_and_write_fifo fail with %d\n", succ);
//        }
        need_ack = true;
    }else{
        printf_("CMD_UNKNOWN\n");
        one_cmd_pkg_ready = false;
        return 4;
    }
    // 收到指令后电机回复ACK，如果是广播操作，不回复
    // 如果需要确认指令接收情况，可以检查cmd_count
    if(need_ack && id != 0){
        // 响应数据结构（固定5字节）
        const uint8_t pkg_size = 5;
        rs485_tx[0] = 0xff;
        rs485_tx[1] = 0xff;
        rs485_tx[2] = 5;
        rs485_tx[3] = 0xff;
        rs485_tx[4] = crc8(rs485_tx, pkg_size - 1);
        // 等待足够长的时间，以便主机完成485方向切换
        sleep_until(uart_rx_time + RX_DELAY);
        uart_write_none_blocking(rs485_tx, pkg_size);
    }
    one_cmd_pkg_ready = false;
    return 0;
}
